/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package helperClasses;

import graphicslib3D.*;

/**
 * Implements methods for manipulating a Quaternion
 * @author Matthew
 */
public class Quaternion 
{
    private float realPart;
    private Vector3D vectorPart;
    
    /**
     * Creates a Quaternion Class
     */
    public Quaternion()
    {
        realPart = 1;
        vectorPart = new Vector3D(0,0,0);
    }
    
    /**
     * Creates a Quaternion Class
     * @param pitch angle in degrees in the x-direction. Is applied first
     * @param yaw angle in degrees in the y-direction. Is applied second
     * @param roll angle in degrees in the z-direction.  Is applied third
     */
    public Quaternion(float pitch, float yaw, float roll)
    {
        Quaternion qPitch = new Quaternion();
        Quaternion qYaw = new Quaternion();
        Quaternion qRoll = new Quaternion();
        Quaternion newQ = new Quaternion();
        
        qPitch.setRealPart((float)Math.cos(Math.toRadians(pitch/2)));
        qPitch.setVectorPart(new Vector3D(Math.sin(Math.toRadians(pitch/2)), 0, 0));
        qPitch.normalize();
        
        qYaw.setRealPart((float)Math.cos(Math.toRadians(yaw/2)));
        qYaw.setVectorPart(new Vector3D(0, Math.sin(Math.toRadians(yaw/2)), 0));
        qYaw.normalize();
        
        qRoll.setRealPart((float)Math.cos(Math.toRadians(roll/2)));
        qRoll.setVectorPart(new Vector3D(0, 0, Math.sin(Math.toRadians(roll/2))));
        qRoll.normalize();
        
        newQ = qPitch.mult(qYaw.mult(qRoll));
        newQ.normalize();
        this.setRealPart(newQ.getRealPart());
        this.setVectorPart(newQ.getVectorPart());
    }
    
    /**
     * Creates a Quaternion Class
     * @param angleAxis array of floats specifying an angle rotated about an axis {angle in degrees, VectorX, VectorY, VectorZ}
     */
    public Quaternion(float[] angleAxis)
    {
        float angleInRadians = (float)Math.toRadians(angleAxis[0]);
        Vector3D axisVector = new Vector3D(angleAxis[1], angleAxis[2], angleAxis[3]);
        
        this.setRealPart((float)Math.cos(angleInRadians/2));
        this.setVectorPart(axisVector.mult(Math.sin(angleInRadians/2)));
        this.normalize();
    }
    
    /**
     * Returns a new Quaternion from a specified angle-axis
     * @param axis axis to rotate about
     * @param angle angle to rotate about axis in degrees
     * @return
     */
    public static Quaternion createFromAngleAxis(Vector3D axis, float angle)
    {
        return new Quaternion(new float[] {(float)axis.getX(), (float)axis.getY(), (float)axis.getZ(), angle});
    }
    
    /**
     * Gets the real component of the Quaternion
     * @return 
     */
    public float getRealPart()
    {
        return realPart;
    }
    
    /**
     * Sets the real component of the Quaternion
     * @param realPart
     */
    public void setRealPart(float realPart)
    {
        this.realPart = realPart;
    }
    
    /**
     * Gets the Vector component of the Quaternion
     * @return
     */
    public Vector3D getVectorPart()
    {
        return vectorPart;
    }
    
    /**
     * Sets the Vector component of the Quaternion
     * @param vectorPart
     */
    public void setVectorPart(Vector3D vectorPart)
    {
        this.vectorPart = vectorPart;
    }
    
    /**
     * Gets the magnitude of the Quaternion
     * @return
     */
    public float getMagnitude()
    {
        //double sum = (realPart * realPart) + (vectorPart.getX() * vectorPart.getX()) + (vectorPart.getY() * vectorPart.getY()) + (vectorPart.getZ() * vectorPart.getZ());
        return (float)Math.sqrt(getMagnitudeSquared());
    }
    
    /**
     * Gets the magnitude before the square root has been applied
     * @return
     */
    private float getMagnitudeSquared()
    {
        return (float)((realPart * realPart) + (vectorPart.getX() * vectorPart.getX()) + (vectorPart.getY() * vectorPart.getY()) + (vectorPart.getZ() * vectorPart.getZ()));
    }
    
    /**
     * Normalizes the Quaternion if it needs it.
     */
    public void normalize()
    {
        if (getMagnitudeSquared() < 0.00000001)
        {
            return;
        }
        if (Math.abs(getMagnitudeSquared()-1) > 0.001) 
        {
            float magnitude = getMagnitude();
            realPart = realPart/magnitude;
            vectorPart.setX(vectorPart.getX()/magnitude);
            vectorPart.setY(vectorPart.getY()/magnitude);
            vectorPart.setZ(vectorPart.getZ()/magnitude);
        }
    }
    
    /**
     * Returns a Quaternion multiplied by a scalar
     * @param value
     * @return a Quaternion
     */
    public Quaternion scale(float value)
    {
        Quaternion q = new Quaternion();
        q.setRealPart(this.getRealPart() * value);
        q.setVectorPart(this.getVectorPart().mult(value));
        return q;
    }
    
    /**
     * Returns a Quaternion of two Quaternions added together
     * @param q
     * @return a quaternion
     */
    public Quaternion add(Quaternion q)
    {
        Quaternion ret = new Quaternion();
        ret.setRealPart(this.getRealPart() + q.getRealPart());
        ret.setVectorPart(this.getVectorPart().add(q.getVectorPart()));
        return ret;
    }
    
    /**
     * Performs dot multiplication with another Quaternion
     * @param quaternion
     * @return
     */
    public float dot(Quaternion quaternion)
    {
        return (float)((realPart * quaternion.getRealPart()) + (vectorPart.getX() * quaternion.getVectorPart().getX()) + (vectorPart.getY() * quaternion.getVectorPart().getY()) + (vectorPart.getZ() * quaternion.getVectorPart().getZ()));
    }
    
    /**
     * Multiplies two Quarternions together
     * @param quaternion
     * @return
     */
    public Quaternion mult(Quaternion quaternion)
    {
        Quaternion qRet = new Quaternion();
        qRet.setRealPart(this.getRealPart() * quaternion.getRealPart() - (float)(this.getVectorPart().dot(quaternion.getVectorPart())));
        Vector3D v1, v2, v3;
        v1 = quaternion.getVectorPart().mult(this.getRealPart());
        v2 = this.getVectorPart().mult(quaternion.getRealPart());
        v3 = this.getVectorPart().cross(quaternion.getVectorPart());
        qRet.setVectorPart(v1.add(v2.add(v3)));
        //qRet.setVectorPart(quaternion.getVectorPart().mult(this.getRealPart()).add(this.getVectorPart().mult(quaternion.getRealPart())).add(this.getVectorPart().cross(quaternion.getVectorPart())));
        return qRet;
    }
    
    /**
     * Returns the angle axis representation of the Quaternion
     * @return
     */
    public float[] getAngleAxis()
    {
        float angle = 2 * (float)Math.acos(this.getRealPart());
        float denom = (float)Math.sin(angle/2);
        
        if (Math.abs(denom) < 0.0001)
        {
            denom = 1;
        }
        Vector3D axis = new Vector3D(this.getVectorPart().getX()/denom, this.getVectorPart().getY()/denom, this.getVectorPart().getZ()/denom);//.normalize();
        float[] angleAxis = {(float)Math.toDegrees(angle), (float)axis.getX(), (float)axis.getY(), (float)axis.getZ()};
                
        return angleAxis;
    }
}
